/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package cageballserver.physics;

import cageballcommon.messages.Action;
import cageballcommon.messages.InitPlayer;
import cageballcommon.messages.InitTeam;
import cageballcommon.messages.Player;
import cageballcommon.messages.PlayerAction;
import cageballcommon.enummerations.TeamEnum;
import cageballcommon.messages.Ball;
import cageballcommon.messages.Info;
import cageballcommon.messages.State;
import cageballcommon.messages.Team;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.Properties;
import java.util.Random;
import net.phys2d.math.Vector2f;
import net.phys2d.raw.Body;
import net.phys2d.raw.StaticBody;
import net.phys2d.raw.World;
import net.phys2d.raw.shapes.Box;
import net.phys2d.raw.shapes.Circle;
import net.phys2d.raw.strategies.QuadSpaceStrategy;

/**
 *
 * @author kanonas
 */
public class Phys2d implements PhysicsEngine {

    private Properties physicsProperties;
    private Properties gameProperties;
    private InitTeam team1;
    private InitTeam team2;
    private World world;
    private Body ball;
    private long time;
    private HashMap<String, PlayerState> map;
    private int stepCount;
    private float ballForceX;
    private float ballForceY;
    private float stopRadius;
    private float forceRadius;
    private float playerMaxForce;
    private float playerMinForce;
    private float kickRadius;
    private float kickError;
    private float kickForce;
    private int kickDelay;
    private float lengthX;
    private float lengthY;

    public Phys2d(Properties physicsProperties) {
        this.physicsProperties = physicsProperties;
        ballForceX = 0;
        ballForceY = 0;
        stepCount = Integer.parseInt(physicsProperties.getProperty("stepCount"));
        stopRadius = Float.parseFloat(physicsProperties.getProperty("stopRadius"));
        forceRadius = Float.parseFloat(physicsProperties.getProperty("forceRadius"));
        playerMaxForce = Float.parseFloat(physicsProperties.getProperty("playerMaxForce"));
        playerMinForce = Float.parseFloat(physicsProperties.getProperty("playerMinForce"));
        kickForce = Float.parseFloat(physicsProperties.getProperty("kickForce"));

        ball = null;

        map = new HashMap<String, PlayerState>();
    }

    public void init(Properties gameProperties, InitTeam team1, InitTeam team2) {

        this.gameProperties = gameProperties;
        this.team1 = team1;
        this.team2 = team2;

        float playerSize = Float.parseFloat(gameProperties.getProperty("playerSize"));
        float ballSize = Float.parseFloat(gameProperties.getProperty("ballSize"));
        float postSize = Float.parseFloat(gameProperties.getProperty("postSize"));

        lengthX = Float.parseFloat(gameProperties.getProperty("fieldLengthX"));
        lengthY = Float.parseFloat(gameProperties.getProperty("fieldLengthY"));

        float goalSize = Float.parseFloat(gameProperties.getProperty("goalSize"));

        float playerFriction = Float.parseFloat(physicsProperties.getProperty("playerFriction"));
        float ballFriction = Float.parseFloat(physicsProperties.getProperty("ballFriction"));

        float playerMass = Float.parseFloat(physicsProperties.getProperty("playerMass"));
        float ballMass = Float.parseFloat(physicsProperties.getProperty("ballMass"));

        float wallBounce = Float.parseFloat(physicsProperties.getProperty("wallBounce"));
        float postBounce = Float.parseFloat(physicsProperties.getProperty("postBounce"));
        float ballBounce = Float.parseFloat(physicsProperties.getProperty("ballBounce"));

        kickRadius = Float.parseFloat(gameProperties.getProperty("kickRadius"));
        kickError = Float.parseFloat(gameProperties.getProperty("kickError"));
        kickDelay = Integer.parseInt(gameProperties.getProperty("kickDelay"));

        world = new World(new Vector2f(0.0f, 10.0f), 10, new QuadSpaceStrategy(20, 5));
        world.clear();
        world.setGravity(0, 0);

        Body wall1 = new StaticBody("Ground1", new Box(11.0f, (lengthY - goalSize) / 2));
        Body wall2 = new StaticBody("Ground2", new Box(11.0f, (lengthY - goalSize) / 2));
        Body wall3 = new StaticBody("Ground1", new Box(11.0f, (lengthY - goalSize) / 2));
        Body wall4 = new StaticBody("Ground2", new Box(11.0f, (lengthY - goalSize) / 2));
        Body wall5 = new StaticBody("Ground3", new Box(lengthX, 11.0f));
        Body wall6 = new StaticBody("Ground3", new Box(lengthX, 11.0f));

        wall1.setPosition(-6, (lengthY - goalSize) / 4);
        wall2.setPosition(-6, lengthY - (lengthY - goalSize) / 4);
        wall3.setPosition(lengthX + 5, (lengthY - goalSize) / 4);
        wall4.setPosition(lengthX + 5, lengthY - (lengthY - goalSize) / 4);
        wall5.setPosition(lengthX / 2, 17);
        wall6.setPosition(lengthX / 2, lengthY + 5);

        wall1.setRestitution(wallBounce);
        wall2.setRestitution(wallBounce);
        wall3.setRestitution(wallBounce);
        wall4.setRestitution(wallBounce);
        wall5.setRestitution(wallBounce);
        wall6.setRestitution(wallBounce);

        world.add(wall1);
        world.add(wall2);
        world.add(wall3);
        world.add(wall4);
        world.add(wall5);
        world.add(wall6);

        Body goalBlocker1 = new StaticBody("GoalBlocker1", new Box(11.0f, goalSize));
        Body goalBlocker2 = new StaticBody("GoalBlocker1", new Box(11.0f, goalSize));

        goalBlocker1.setPosition(-6, lengthY / 2);
        goalBlocker2.setPosition(lengthX + 5, lengthY / 2);

        goalBlocker1.setRestitution(wallBounce);
        goalBlocker2.setRestitution(wallBounce);

        world.add(goalBlocker1);
        world.add(goalBlocker2);

        Body post1 = new StaticBody("Post1", new Circle(postSize));
        Body post2 = new StaticBody("Post2", new Circle(postSize));
        Body post3 = new StaticBody("Post3", new Circle(postSize));
        Body post4 = new StaticBody("Post4", new Circle(postSize));

        post1.setPosition(0, lengthY / 2 + goalSize / 2);
        post2.setPosition(0, lengthY / 2 - goalSize / 2);
        post3.setPosition(lengthX - 1, lengthY / 2 + goalSize / 2);
        post4.setPosition(lengthX - 1, lengthY / 2 - goalSize / 2);

        post1.setRestitution(postBounce);
        post2.setRestitution(postBounce);
        post3.setRestitution(postBounce);
        post4.setRestitution(postBounce);

        world.add(post1);
        world.add(post2);
        world.add(post3);
        world.add(post4);

        ball = new Body("Ball", new Circle(ballSize), ballMass);

        ball.setRestitution(ballBounce);
        ball.setDamping(ballFriction);
        ball.setRotatable(false);
        ball.addExcludedBody(goalBlocker1);
        ball.addExcludedBody(goalBlocker2);
        world.add(ball);

        for (InitPlayer p : team1.getPlayers()) {
            Body body = new Body("Team1", new Circle(playerSize), playerMass);
            body.setDamping(playerFriction);
            PlayerState playerState = new PlayerState(body, team1.getId(), p.getId(), p.getXStart(), p.getYStart());
            body.setRotatable(false);
            map.put(playerHash(team1.getId(), p.getId()), playerState);
            world.add(body);
            body.addExcludedBody(ball);
        }

        for (InitPlayer p : team2.getPlayers()) {
            Body body = new Body("Team2", new Circle(playerSize), playerMass);
            body.setDamping(playerFriction);
            PlayerState playerState = new PlayerState(body, team2.getId(), p.getId(), p.getXStart(), p.getYStart());
            body.setRotatable(false);
            map.put(playerHash(team2.getId(), p.getId()), playerState);
            world.add(body);
            body.addExcludedBody(ball);
        }

        time = 0;

        reset(TeamEnum.otherTeam);
    }

    public void reset(TeamEnum goal) {

        for (PlayerState p : map.values()) {
            p.getBody().setPosition(p.getStartX(), p.getStartY());
        }

        ball.adjustVelocity(new Vector2f(-ball.getVelocity().getX(), -ball.getVelocity().getY()));

        if (goal == TeamEnum.myTeam) {
            ball.setPosition(lengthX / 4 * 3, lengthY / 2);
        } else if (goal == TeamEnum.otherTeam) {
            ball.setPosition(lengthX / 4, lengthY / 2);
        }
    }

    public void step() {

        Vector2f pos;
        Vector2f dest;
        Vector2f direction;
        float distance;
        float force;

        Vector2f ballVec = new Vector2f(ballForceX, ballForceY);



        if (ballVec.length() != 0) {
            if (ballVec.length() > 1) {
                ballVec.normalise();
            }

            ball.adjustVelocity(new Vector2f(-ball.getVelocity().getX(), -ball.getVelocity().getY()));

            ballVec.scale(kickForce);

            ball.setForce(ballVec.getX(), ballVec.getY());

            ballForceX = 0;
            ballForceY = 0;
        }

        for (int n = 0; n < stepCount; n++) {
            for (PlayerState playerState : map.values()) {

                pos = new Vector2f(playerState.getBody().getPosition().getX(), playerState.getBody().getPosition().getY());
                dest = new Vector2f(playerState.getDestX(), playerState.getDestY());
                distance = pos.distance(dest);

                if (distance > stopRadius) {
                    pos.sub(dest);
                    direction = pos.negate();
                    direction.normalise();

                    force = distance / forceRadius * playerMaxForce;
                    if (force < playerMinForce) {
                        force = playerMinForce;
                    }
                    if (force > playerMaxForce) {
                        force = playerMaxForce;
                    }

                    direction.scale(force);

                    playerState.getBody().setForce(direction.getX(), direction.getY());
                }
            }

            world.step();
        }

        time++;
    }

    public void addAction(Action action) {
        PlayerState playerState = null;


        for (PlayerAction playerAction : action.getActions()) {

            playerState = getPlayer(action.getTeam(), playerAction.getId());

            if (playerState == null) {
                continue;
            }

            if (playerAction.hasMoveX()) {
                playerState.setDestX(playerAction.getMoveX());
            }

            if (playerAction.hasMoveY()) {
                playerState.setDestY(playerAction.getMoveY());
            }

            if (playerAction.hasKick() && canKick(playerState)) {
                Vector2f ballVector = new Vector2f(ball.getPosition().getX(), ball.getPosition().getY());
                Vector2f forceVector = new Vector2f(playerAction.getKickX(), playerAction.getKickY());
                forceVector.sub(ballVector);
                forceVector.normalise();

                forceVector = newTarget(forceVector, playerAction.getKickStrength(), kickError);

                ballForceX += forceVector.getX();
                ballForceY += forceVector.getY();

                playerState.setLastKick(time);
            }
        }
    }

    public State getState() {
        ArrayList<Player> team1Players = new ArrayList<Player>();
        ArrayList<Player> team2Players = new ArrayList<Player>();

        Player tmp;

        for (PlayerState playerState : map.values()) {
            tmp = new Player(playerState.getId(), playerState.getBody().getPosition().getX(), playerState.getBody().getPosition().getY());
            if (playerState.getTeam() == team1.getId()) {
                team1Players.add(tmp);
            } else if (playerState.getTeam() == team2.getId()) {
                team2Players.add(tmp);
            }

        }

        Team myTeam = new Team(team1.getId(), team1Players);
        Team otherTeam = new Team(team2.getId(), team2Players);
        State state;
        Ball ballObj = new Ball(ball.getPosition().getX(), ball.getPosition().getY());

        state = new State(time, ballObj, myTeam, otherTeam);

        return state;
    }

    private boolean canKick(PlayerState playerState) {
        Vector2f ballVec = new Vector2f(ball.getPosition().getX(), ball.getPosition().getY());
        Vector2f playerVec = new Vector2f(playerState.getBody().getPosition().getX(), playerState.getBody().getPosition().getY());

        if (playerState.getLastKick() + kickDelay > time) {
            return false;
        }

        return playerVec.distance(ballVec) <= kickRadius;
    }

    private PlayerState getPlayer(int team, int id) {
        return map.get(playerHash(team, id));
    }

    private String playerHash(int team, int id) {
        return team + ":" + id;
    }

    private Vector2f getVector(float angle) {
        Vector2f tmp = new Vector2f((float) Math.cos(angle), (float) Math.sin(angle));
        tmp.normalise();
        return tmp;
    }

    private Vector2f newTarget(Vector2f in, float strength, float interval) {

        float angle = getAngle(in);

        Random rand = new Random();

        float diff = 2 * interval * rand.nextFloat() - interval;

        float newAngle = (angle + diff * strength);

        Vector2f tmp = getVector(newAngle);

        tmp.normalise();
        tmp.scale(strength);

        return tmp;
    }

    private float getAngle(Vector2f in) {
        javax.vecmath.Vector2f v = new javax.vecmath.Vector2f(in.getX(), in.getY());
        v.normalize();

        javax.vecmath.Vector2f normal = new javax.vecmath.Vector2f(1, 0);
        normal.normalize();

        float angle = v.angle(normal);

        if (in.getY() < 0) {
            angle = (float) Math.PI + (float) Math.PI - angle;
        }

        return angle;
    }
}
